#include "mklm_command_impl.h"
#include <QJsonDocument>
#include <QJsonObject>
#include <QJsonArray>
#include <QUuid>
#include <QHostAddress>

namespace infrastructure {
namespace vehicle {
namespace mklm {

MklmCommandImpl::MklmCommandImpl(std::shared_ptr<kernel::ILogger> logger, QObject* parent)
    : QObject(parent)
    , m_logger(logger)
{
    if (m_logger) {
        m_logger->info("MklmCommandImpl initialized");
    }
}

MklmCommandImpl::~MklmCommandImpl()
{
    m_agvConnections.clear();
    if (m_logger) {
        m_logger->info("MklmCommandImpl destroyed");
    }
}

bool MklmCommandImpl::registerAgv(int agvNumber, const QString& ipAddress)
{
    if (ipAddress.isEmpty()) {
        if (m_logger) {
            m_logger->error(QString("Cannot register AGV %1 with empty IP address").arg(agvNumber));
        }
        return false;
    }

    AgvConnection conn;
    conn.ipAddress = ipAddress;

    // 创建 task_server 连接到 AGV:29204（发送命令）
    conn.taskServer = std::make_unique<robokit::RProtocol>(this);
    if (!conn.taskServer->initTcpConnection(QHostAddress(ipAddress), 29204)) {
        if (m_logger) {
            m_logger->error(QString("Failed to connect task_server to AGV %1 at %2:29204")
                .arg(agvNumber).arg(ipAddress));
        }
        return false;
    }

    // 创建 robo_task 连接到 AGV:19206（RobotTask API）
    conn.roboTask = std::make_unique<robokit::RobotTask>();
    if (!conn.roboTask->initTcpConnection(QHostAddress(ipAddress), 19206)) {
        if (m_logger) {
            m_logger->error(QString("Failed to connect robo_task to AGV %1 at %2:19206")
                .arg(agvNumber).arg(ipAddress));
        }
        return false;
    }

    m_agvConnections[agvNumber] = std::move(conn);

    if (m_logger) {
        m_logger->info(QString("AGV %1 command connections established (IP: %2)")
            .arg(agvNumber).arg(ipAddress));
    }

    return true;
}

void MklmCommandImpl::unregisterAgv(int agvNumber)
{
    auto it = m_agvConnections.find(agvNumber);
    if (it != m_agvConnections.end()) {
        m_agvConnections.erase(it);
        if (m_logger) {
            m_logger->info(QString("AGV %1 command connections removed").arg(agvNumber));
        }
    }

    // 清理MKLM特有配置
    m_finalOperations.erase(agvNumber);
}

// ========== MKLM特有功能实现 ==========

void MklmCommandImpl::setFinalOperation(int agvNumber, const std::string& stationId, const std::string& operationName)
{
    m_finalOperations[agvNumber] = {stationId, operationName};
    if (m_logger) {
        m_logger->debug(QString("AGV %1 final operation set: %2 at %3")
            .arg(agvNumber)
            .arg(QString::fromStdString(operationName))
            .arg(QString::fromStdString(stationId)));
    }
}

void MklmCommandImpl::clearFinalOperation(int agvNumber)
{
    m_finalOperations.erase(agvNumber);
    if (m_logger) {
        m_logger->debug(QString("AGV %1 final operation cleared").arg(agvNumber));
    }
}

void MklmCommandImpl::setDoorConfigMap(const std::map<std::pair<std::string, std::string>, int>& doorIds)
{
    m_doorConfigMap = doorIds;
    if (m_logger) {
        m_logger->info(QString("Door config map updated: %1 entries").arg(doorIds.size()));
    }
}

MklmCommandImpl::AgvConnection* MklmCommandImpl::getConnection(int agvNumber)
{
    auto it = m_agvConnections.find(agvNumber);
    if (it == m_agvConnections.end()) {
        if (m_logger) {
            m_logger->warning(QString("No connection found for AGV %1").arg(agvNumber));
        }
        return nullptr;
    }
    return &it->second;
}

// ============================================================================
// 命令实现（纯发送逻辑，无业务判断）
// 业务逻辑（路径比对、状态检查等）应在 VehicleService 中实现
// ============================================================================

/**
 * @brief 移动到站点列表
 * @details 参考 agv.cpp:229-273 的 JSON 构造部分（不包含业务逻辑判断）
 *          使用内部的 m_finalOperations 和 m_doorConfigMap（MKLM特有）
 *
 * 【VehicleService 应实现的业务逻辑】：
 * 1. 检查 AGV 是否在导航中 (task_status == 2/3 && task_type == 3)
 * 2. 路径比对：与 unfinished_path 对比，相同则不下发
 * 3. 处理 finished_path：计算 route_star_index，截取正确的 stations
 * 4. 调用 setFinalOperation() 设置操作（如果需要）
 * 5. 调用 setDoorConfigMap() 设置门配置（全局，启动时设置一次）
 * 6. 更新 AGV 的 last_busy, last_send_move_task_time
 */
kernel::Result<void> MklmCommandImpl::moveToStationList(
    int agvNumber,
    const std::vector<std::string>& stations,
    const std::string& operation)
{
    auto conn = getConnection(agvNumber);
    if (!conn) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV connection not found"
        );
    }

    if (stations.size() < 2) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::InvalidParameter,
            "Station list size must be at least 2"
        );
    }

    // 构建 JSON（参考 agv.cpp:229-259）
    QJsonObject rootObject;
    QJsonArray moveTaskListArray;

    // 查找当前AGV的最终操作配置（MKLM特有）
    auto opIt = m_finalOperations.find(agvNumber);
    bool hasFinalOp = (opIt != m_finalOperations.end());

    for (size_t i = 0; i < stations.size() - 1; ++i) {
        QString sourceId = QString::fromStdString(stations[i]);
        QString targetId = QString::fromStdString(stations[i + 1]);

        QJsonObject moveTaskObj;
        moveTaskObj.insert("id", targetId);
        moveTaskObj.insert("source_id", sourceId);
        moveTaskObj.insert("task_id", QUuid::createUuid().toString().remove("{").remove("}").remove("-"));

        // 添加最终操作：优先使用参数传入的operation，其次使用setFinalOperation设置的
        bool isLastTask = (i == stations.size() - 2);
        if (isLastTask && !operation.empty()) {
            moveTaskObj.insert("operation", QString::fromStdString(operation));
        } else if (hasFinalOp && targetId.toStdString() == opIt->second.stationId) {
            moveTaskObj.insert("operation", QString::fromStdString(opIt->second.operationName));
        }

        // 添加门控制（参考 agv.cpp:248-256）
        auto doorKey = std::make_pair(stations[i], stations[i + 1]);
        auto doorIt = m_doorConfigMap.find(doorKey);
        if (doorIt != m_doorConfigMap.end() && doorIt->second != 0) {
            moveTaskObj.insert("door_id", doorIt->second);
        }

        moveTaskListArray.append(moveTaskObj);
    }

    rootObject.insert("move_task_list", moveTaskListArray);

    // 使用完后清除最终操作（一次性）
    if (hasFinalOp) {
        m_finalOperations.erase(agvNumber);
    }

    // 调用 robo_task API（参考 agv.cpp:263）
    conn->roboTask->robot_task_gotargetlist_req_get(rootObject);

    if (m_logger) {
        m_logger->debug(QString("AGV %1 move task list: %2 stations")
            .arg(agvNumber).arg(stations.size()));
    }

    return kernel::Result<void>::success();
}

/**
 * @brief 移动到单个站点
 * @details 参考 agv.cpp:276-288 PublishMoveTask()
 */
kernel::Result<void> MklmCommandImpl::moveToStation(
    int agvNumber, const std::string& sourceId, const std::string& targetId)
{
    auto conn = getConnection(agvNumber);
    if (!conn) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV connection not found"
        );
    }

    // 构建 JSON（参考 agv.cpp:279-284）
    QJsonObject rootObject;
    rootObject.insert("id", QString::fromStdString(targetId));
    rootObject.insert("source_id", QString::fromStdString(sourceId));

    QJsonDocument jsDoc(rootObject);
    QByteArray jsonBuffer = jsDoc.toJson();

    // 发送命令（参考 agv.cpp:285）
    bool success = conn->taskServer->WriteTcpData(CMD_MOVE_TASK, jsonBuffer,
        [this, agvNumber](uint16_t, QByteArray&) {
            if (m_logger) {
                m_logger->debug(QString("AGV %1 move task sent").arg(agvNumber));
            }
        });

    if (!success) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::NetworkError,
            "Failed to send move task command"
        );
    }

    if (m_logger) {
        m_logger->info(QString("AGV %1 move task: %2 -> %3")
            .arg(agvNumber)
            .arg(QString::fromStdString(sourceId))
            .arg(QString::fromStdString(targetId)));
    }

    return kernel::Result<void>::success();
}

/**
 * @brief 取消任务
 * @details 参考 agv.cpp:334-344 ClearTask()
 */
kernel::Result<void> MklmCommandImpl::cancelTask(int agvNumber)
{
    auto conn = getConnection(agvNumber);
    if (!conn) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV connection not found"
        );
    }

    // 空 JSON（参考 agv.cpp:338-340）
    QJsonObject rootObject;
    QJsonDocument jsDoc(rootObject);
    QByteArray jsonBuffer = jsDoc.toJson();

    // 发送命令（参考 agv.cpp:341）
    bool success = conn->taskServer->WriteTcpData(CMD_CANCEL_TASK, jsonBuffer,
        [this, agvNumber](uint16_t, QByteArray&) {
            if (m_logger) {
                m_logger->debug(QString("AGV %1 cancel task sent").arg(agvNumber));
            }
        });

    if (!success) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::NetworkError,
            "Failed to send cancel task command"
        );
    }

    if (m_logger) {
        m_logger->info(QString("AGV %1 task canceled").arg(agvNumber));
    }

    return kernel::Result<void>::success();
}

/**
 * @brief 暂停任务
 * @details 参考 agv.cpp:418-424 pauseTask()
 */
kernel::Result<void> MklmCommandImpl::pauseTask(int agvNumber)
{
    auto conn = getConnection(agvNumber);
    if (!conn) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV connection not found"
        );
    }

    // 空数据（参考 agv.cpp:421-422）
    QByteArray emptyData;

    bool success = conn->taskServer->WriteTcpData(CMD_PAUSE_TASK, emptyData,
        [this, agvNumber](uint16_t, QByteArray&) {
            if (m_logger) {
                m_logger->debug(QString("AGV %1 pause task sent").arg(agvNumber));
            }
        });

    if (!success) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::NetworkError,
            "Failed to send pause task command"
        );
    }

    if (m_logger) {
        m_logger->info(QString("AGV %1 task paused").arg(agvNumber));
    }

    return kernel::Result<void>::success();
}

/**
 * @brief 恢复任务
 * @details 参考 agv.cpp:425-431 resumeTask()
 */
kernel::Result<void> MklmCommandImpl::resumeTask(int agvNumber)
{
    auto conn = getConnection(agvNumber);
    if (!conn) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV connection not found"
        );
    }

    // 空数据（参考 agv.cpp:428-429）
    QByteArray emptyData;

    bool success = conn->taskServer->WriteTcpData(CMD_RESUME_TASK, emptyData,
        [this, agvNumber](uint16_t, QByteArray&) {
            if (m_logger) {
                m_logger->debug(QString("AGV %1 resume task sent").arg(agvNumber));
            }
        });

    if (!success) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::NetworkError,
            "Failed to send resume task command"
        );
    }

    if (m_logger) {
        m_logger->info(QString("AGV %1 task resumed").arg(agvNumber));
    }

    return kernel::Result<void>::success();
}

/**
 * @brief 控制顶升
 * @details 参考 agv.cpp:347-358 PublishJack()
 */
kernel::Result<void> MklmCommandImpl::controlJack(int agvNumber, bool raise)
{
    auto conn = getConnection(agvNumber);
    if (!conn) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV connection not found"
        );
    }

    // 构建 JSON（参考 agv.cpp:349-352）
    QJsonObject rootObject;
    QString operation = raise ? "JackHeight" : "JackDown";
    rootObject.insert("operation", operation);

    QJsonDocument jsDoc(rootObject);
    QByteArray jsonBuffer = jsDoc.toJson();

    // 发送命令（参考 agv.cpp:355）
    bool success = conn->taskServer->WriteTcpData(CMD_JACK_CONTROL, jsonBuffer,
        [this, agvNumber](uint16_t, QByteArray&) {
            if (m_logger) {
                m_logger->debug(QString("AGV %1 jack control sent").arg(agvNumber));
            }
        });

    if (!success) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::NetworkError,
            "Failed to send jack control command"
        );
    }

    if (m_logger) {
        m_logger->info(QString("AGV %1 jack control: %2")
            .arg(agvNumber).arg(raise ? "raise" : "lower"));
    }

    return kernel::Result<void>::success();
}

/**
 * @brief 开始充电
 * @details 参考 agv.cpp:360-369 PublishCharge()
 */
kernel::Result<void> MklmCommandImpl::startCharging(int agvNumber, const std::string& chargePoint)
{
    auto conn = getConnection(agvNumber);
    if (!conn) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV connection not found"
        );
    }

    // 构建 JSON（参考 agv.cpp:362-363）
    QJsonObject rootObject;
    rootObject.insert("charge_point", QString::fromStdString(chargePoint));

    QJsonDocument jsDoc(rootObject);
    QByteArray jsonBuffer = jsDoc.toJson();

    // 发送命令（参考 agv.cpp:366）
    bool success = conn->taskServer->WriteTcpData(CMD_START_CHARGE, jsonBuffer,
        [this, agvNumber](uint16_t, QByteArray&) {
            if (m_logger) {
                m_logger->debug(QString("AGV %1 start charge sent").arg(agvNumber));
            }
        });

    if (!success) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::NetworkError,
            "Failed to send start charge command"
        );
    }

    if (m_logger) {
        m_logger->info(QString("AGV %1 start charging at %2")
            .arg(agvNumber).arg(QString::fromStdString(chargePoint)));
    }

    return kernel::Result<void>::success();
}

/**
 * @brief 停止充电
 * @details 参考 agv.cpp:382-391 PublishQuitCharge()
 */
kernel::Result<void> MklmCommandImpl::stopCharging(int agvNumber)
{
    auto conn = getConnection(agvNumber);
    if (!conn) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV connection not found"
        );
    }

    // 构建 JSON（参考 agv.cpp:384-385）
    QJsonObject rootObject;
    rootObject.insert("charge_point", "");

    QJsonDocument jsDoc(rootObject);
    QByteArray jsonBuffer = jsDoc.toJson();

    // 发送命令（参考 agv.cpp:388）
    bool success = conn->taskServer->WriteTcpData(CMD_STOP_CHARGE, jsonBuffer,
        [this, agvNumber](uint16_t, QByteArray&) {
            if (m_logger) {
                m_logger->debug(QString("AGV %1 stop charge sent").arg(agvNumber));
            }
        });

    if (!success) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::NetworkError,
            "Failed to send stop charge command"
        );
    }

    if (m_logger) {
        m_logger->info(QString("AGV %1 stop charging").arg(agvNumber));
    }

    return kernel::Result<void>::success();
}

/**
 * @brief 控制门
 * @details 参考 agv.cpp:371-381 publishDoorControl()
 */
kernel::Result<void> MklmCommandImpl::controlDoor(int agvNumber, int doorId, bool open)
{
    auto conn = getConnection(agvNumber);
    if (!conn) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV connection not found"
        );
    }

    // 构建 JSON（参考 agv.cpp:373-375）
    QJsonObject rootObject;
    rootObject.insert("door_id", doorId);
    rootObject.insert("operation", open);

    QJsonDocument jsDoc(rootObject);
    QByteArray jsonBuffer = jsDoc.toJson();

    // 发送命令（参考 agv.cpp:378）
    bool success = conn->taskServer->WriteTcpData(CMD_DOOR_CONTROL, jsonBuffer,
        [this, agvNumber](uint16_t, QByteArray&) {
            if (m_logger) {
                m_logger->debug(QString("AGV %1 door control sent").arg(agvNumber));
            }
        });

    if (!success) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::NetworkError,
            "Failed to send door control command"
        );
    }

    if (m_logger) {
        m_logger->info(QString("AGV %1 door control: doorId=%2, open=%3")
            .arg(agvNumber).arg(doorId).arg(open));
    }

    return kernel::Result<void>::success();
}

/**
 * @brief 请求当前地图
 * @details 参考 agv.cpp:432-463 GetCurMap()
 */
kernel::Result<void> MklmCommandImpl::requestCurrentMap(int agvNumber)
{
    auto conn = getConnection(agvNumber);
    if (!conn) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV connection not found"
        );
    }

    // 空数据（参考 agv.cpp:434-435）
    QByteArray emptyData;

    bool success = conn->taskServer->WriteTcpData(CMD_GET_MAP, emptyData,
        [this, agvNumber](uint16_t, QByteArray& receivedata) {
            // 回调处理（参考 agv.cpp:435-462）
            QJsonDocument itemDoc = QJsonDocument::fromJson(receivedata);
            QJsonObject rootObject = itemDoc.object();

            if (rootObject.value("ret_code").toDouble() != QJsonValue::Undefined) {
                if (m_logger) {
                    m_logger->info(QString("AGV %1 map data received").arg(agvNumber));
                }
                // TODO: 保存地图文件的逻辑
            } else {
                if (m_logger) {
                    m_logger->error(QString("AGV %1 map download failed").arg(agvNumber));
                }
            }
        });

    if (!success) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::NetworkError,
            "Failed to send get map command"
        );
    }

    if (m_logger) {
        m_logger->info(QString("AGV %1 request current map").arg(agvNumber));
    }

    return kernel::Result<void>::success();
}

kernel::Result<void> MklmCommandImpl::switchMap(
    int agvNumber,
    const std::string& mapName,
    const std::string& switchPoint)
{
    auto conn = getConnection(agvNumber);
    if (!conn || !conn->roboTask) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "AGV connection not found");
    }

    QJsonObject rootObject;
    rootObject.insert("operation", QStringLiteral("Script"));
    rootObject.insert("script_name", QStringLiteral("syspy/switchMap.py"));
    rootObject.insert("source_id", QStringLiteral("SELF_POSITION"));
    rootObject.insert("id", QStringLiteral("SELF_POSITION"));
    rootObject.insert("task_id", QUuid::createUuid().toString(QUuid::WithoutBraces));

    QJsonObject argsObject;
    argsObject.insert("map", QString::fromStdString(mapName));
    argsObject.insert("switchPoint", QString::fromStdString(switchPoint));
    rootObject.insert("script_args", argsObject);

    conn->roboTask->robot_task_gotarget_req_get(rootObject);

    if (m_logger) {
        m_logger->info(QString("MKLM AGV %1 switch map -> %2 (point %3)")
                           .arg(agvNumber)
                           .arg(QString::fromStdString(mapName))
                           .arg(QString::fromStdString(switchPoint)));
    }

    return kernel::Result<void>::success();
}

} // namespace mklm
} // namespace vehicle
} // namespace infrastructure
